# 处理GNSS日志
import datetime
import warnings
from time import sleep
import pytz

import numpy as np
import pandas as pd
from scipy import interpolate


class WGS84:
    def __init__(self):
        # 长轴
        self.r = 6378137.0
        # 地球自转速度
        self.omega_e = 7.2921151467e-5
        # WGS84椭球坐标系 第一偏心率
        self.e2 = 0.00669437999013

    def RM(self, phi):
        return self.r * (1 - self.e2) / np.power(1 - self.e2 * np.sin(phi) * np.sin(phi), 1.5)

    def RN(self, phi):
        return self.r / np.sqrt(1 - self.e2 * np.sin(phi) * np.sin(phi))

    def dN(self, lat1, lat2):
        dn = (lat1 - lat2) * self.RM(lat1)
        return dn

    def dE(self, lat1, lon1, lon2):
        de = (lon1 - lon2) * self.RN(lat1) * np.cos(lat1)
        return de

    def ECEF_to_LLA(self, x, y, z):
        """
        ECEF to Lat Lon Altitude
        :param x:
        :param y:
        :param z:
        :return:
        """
        p = np.sqrt(x ** 2 + y ** 2)
        lon = 2.0 * np.arctan2(y, x + p)
        lat = np.arctan(z / (p * (1 - self.e2)))
        h = 0
        circle = 0
        while True:
            RN = self.RN(lat)
            h_pre = h
            h = p / np.cos(lat) - RN
            lat = np.arctan2(z, p * (1 - self.e2 * RN / (RN + h)))
            if np.fabs(h - h_pre) < 1e-8:
                break
            circle += 1
            if circle > 10:
                print("ecef-frame can't be transmit to n-frame")
                break
        return lat, lon, h

    def lla_to_Cne(self, lat, lon):
        sin_lat = np.sin(lat)
        cos_lat = np.cos(lat)
        sin_lon = np.sin(lon)
        cos_lon = np.cos(lon)
        Cne = np.mat([
            [-sin_lat * cos_lon, -sin_lon, -cos_lat * cos_lon],
            [-sin_lat * sin_lon, cos_lon, -cos_lat * sin_lon],
            [cos_lat, 0, -sin_lat]
        ], np.double)
        return Cne


defualt_date = datetime.date.today()


class SplitNmeaData:
    class Option:
        def __init__(self):
            self.gga = False
            self.gsv = False
            self.hdt = False
            self.rmc = False
            self.kxst = False
            self.rangea = False
            self.rangeha = False
            self.file_type = 0  # 0 for bin, 1 for ascii
            self.data_type = 0  # 0 for double 1 for int, valid for binary file
            self.bin_cols = 7
            self.date = defualt_date

    def __init__(self, input_file, opt: Option):
        self.gga_header = ['%6s' % 'week', '%11s' % 'gps_sec', '%16s' % 'lat', '%17s' % 'lon', '%10s' % 'h',
                           '%4s' % 'hdop',
                           '%3s' % 'q', '%8s' % 'ns']
        self.gga_formats = ['%6d ', '%10.3f ', '%3.12f ', '%3.12f ', '%3.5f ', '%6.3f ', '%2d ', '%1d ']

        self.ksxt_header = ['%6s' % 'week', '%11s' % 'gps_sec', '%16s' % 'lat', '%17s' % 'lon', '%10s' % 'h',
                            '%8s' % 'v_n',
                            '%8s' % 'v_e',
                            '%8s' % 'v_u', '%9s' % 'roll', '%9s' % 'pitch',
                            '%9s' % 'yaw', '%5s' % 'p_q', '%5s' % 'h_q', '%5s' % 'ns1', '%5s' % 'ns2']
        self.ksxt_formats = ['%6d ',  # gps-week
                             '%10.3f ',  # gps-second
                             '%3.12f ',  # lat
                             '%3.12f ',
                             '%3.5f ',
                             '%7.3f ',  # v
                             '%7.3f ',  # v
                             '%7.3f ',  # v
                             '%8.3f ',  # atti
                             '%8.3f ',  # atti
                             '%8.3f ',  # atti
                             '%4d ',  # pos_q
                             '%4d ',  # heading q
                             '%4d ',  # ns1
                             '%4d ',  # ns2
                             ]
        self.undulation = None
        self.input_file = input_file
        self.opt = opt
        self.exception = None

    def splitData(self):
        opt = self.opt
        # if opt.file_type == 0:
        #     '''binary file'''
        #     if opt.data_type == 0:
        #         dtype = np.double
        #     else:
        #         dtype = np.int
        #     try:
        #         raw_data = np.fromfile(self.input_file, dtype=dtype)
        #         mat_data = raw_data.reshape([-1, opt.bin_cols])
        #         np.savetxt(self.input_file + ".imutxt", mat_data, fmt="%.15f")
        #     except Exception as err:
        #         self.exception = err
        #         return
        #
        # if opt.file_type == 1:
        '''ascii file'''
        try:
            if opt.gga is True:
                output_file = self.input_file + '.gga'

                self.handle_gga(self.input_file, output_file, date=opt.date)
            if opt.kxst is True:
                output_file = self.input_file + '.kxst'
                self.handle_kxst(self.input_file, output_file)
        except Exception as err:
            self.exception = err
            return

    def handle_kxst(self, input_file, ksxt_output_file, undulation=0):
        line_count = 0
        out_ksxt = open(ksxt_output_file, 'w')
        out_ksxt.write("# file:%s\n" % input_file)
        out_ksxt.write("# p_q: pos quality,0:invalid, 1: SPP, 2: RTK-float, 3: RTK-fixed\n")
        out_ksxt.write("# h_q: heading quality,0:invalid, 1: SPP, 2: RTK-float, 3: RTK-fixed\n")
        out_ksxt.write("# ksxt message from UB432\n")
        self.write_header(self.ksxt_header, out_ksxt)
        if self.undulation is None:
            warnings.warn("No valid GGA message, ubdulation is set to given")
            self.undulation = undulation

        with open(input_file, 'r') as infile:
            while True:
                line = infile.readline()
                if line == '': break
                data = line.split(',')
                if len(data) == 1:
                    continue
                if data[0] == '$KSXT':
                    if data[1] == '' or float(data[2]) < 1: continue
                    self.__write_data_ksxt(data, out_ksxt, self.undulation)
                    line_count += 1
        return line_count

    def handle_gga(self, input_file, gga_output_file, date):
        line_count = 0
        out_gga = open(gga_output_file, 'w')
        out_gga.write("# file:%s\n" % input_file)
        out_gga.write(
            "# q: pos quality,0:invalid, 1: SPP, 2: D-GPS/SBAS, 4: RTK-fixed,5:RTK-float,6:INS,7: fixed position\n")
        out_gga.write("# data type GGA\n")
        self.write_header(self.gga_header, out_gga)
        with open(input_file, 'r') as infile:
            while True:
                line = infile.readline()
                if line == '': break
                data = line.split(',')
                if len(data) == 1:
                    continue
                if data[0].__contains__('GGA'):
                    if data[1] == '' or data[2] == '': continue
                    self.__write_data_gga(data, out_gga, date)
                    line_count += 1
        return line_count

    def __write_data_ksxt(self, data: list, f, undulation=0):
        gps_week, gps_sec = self.__utc2gpst(data[1], mode='ksxt')
        try:
            lat = float(data[3])
            h = float(data[4]) + undulation  # 海拔
            lon = float(data[2])
            v_e = float(data[17]) / 3.6  # km/h->m/s
            v_n = float(data[18]) / 3.6  # km/h->m/s
            v_u = float(data[19]) / 3.6  # km/h->m/s
            roll = 0  # float(data[9])
            pitch = float(data[6])
            heading = float(data[5])
            pos_q = float(data[10])
            heading_q = int(data[11])
            ns1 = float(data[12])
            ns2 = float(data[13])
            self.write_line(
                [gps_week, gps_sec, lat, lon, h, v_e, v_n, v_u, roll, pitch, heading, pos_q, heading_q, ns1, ns2],
                self.ksxt_formats, f)
        except Exception  as e:
            print(e.args)

    def __write_data_gga(self, data: list, f, date):
        try:
            gps_week, gps_sec = self.__utc2gpst(data[1], date, mode='gga', )
            lat = ddmmmm2deg(data[2], 2)
            h = float(data[9]) + float(data[11])  # 海拔转换为大地高
            lon = ddmmmm2deg(data[4], 3)
            q = float(data[6])
            ns = float(data[7])
            hdop = float(data[8])
            self.undulation = float(data[11])
            self.write_line([gps_week, gps_sec, lat, lon, h, hdop,q, ns], self.gga_formats, f)
        except Exception as e:
            print(e.args)

    def write_line(self, data: list, formats: list, f):
        for idx, d in enumerate(data):
            f.write(formats[idx] % (d))
        f.write('\n')

    def write_header(self, header, f):
        for h in header:
            f.write(h)
        f.write('\n')

    def __utc2gpst(self, utc: str, date: datetime = defualt_date, mode='ksxt'):
        """
        utc字符串转换为gps周和秒，问题在于gga的时间戳不包含年月日，因此传入年月日信息date
        :param utc: 字符串
        :param date: 日期
        :param mode:
        :return:
        """
        assert mode == 'ksxt' or mode == 'gga'
        if mode == 'ksxt':
            year = int(utc[0:4])
            mon = int(utc[4:6])
            dd = int(utc[6:8])
            hh = int(utc[8:10])
            mm = int(utc[10:12])
            ss = int(utc[12:14])
            ms = int(utc[15:-1])
        else:  # hh/mm/s.ss
            year = date.year
            mon = date.month
            dd = date.day
            hh = int(utc[0:2])
            mm = int(utc[2:4])
            ss = int(utc[4:6])
            ms = int(utc[7:-1])
        utc_tz = pytz.timezone('UTC')
        cur_time = datetime.datetime(year, mon, dd, hh, mm, ss, ms,tzinfo=utc_tz) + datetime.timedelta(0, 18, 0, 0,
                                                                                         0)  # GPST - UTC0 = 18s
        gps_0_time = datetime.datetime(1980, 1, 6, 0, 0, 0, 0,tzinfo=utc_tz)
        diff_time = cur_time - gps_0_time
        gps_week = np.floor(diff_time.days / 7)
        gps_second = 86400 * (diff_time.days % 7) + diff_time.seconds + diff_time.microseconds / 1000
        return gps_week, gps_second


def ddmmmm2deg(indata: str, d_length: int):
    """
    ddmm.mmmmm转换为deg
    :param indata:
    :param d_length: 2:dd or 3:ddd
    :return:
    """
    dd = int(indata[0:d_length])
    mm = float(indata) - 100 * dd
    return dd + mm / 60


class PositionPlot():
    def __init__(self, input_file, column_index,
                 ref=None,
                 skip_rows=0,
                 delimiter="\s+",
                 max_points=None,
                 bins=None,
                 coor=0  # 0 : navigation system 1 Ecef system xyx
                 ):
        self.input_file = input_file
        self.exception = None
        try:
            data_frame = pd.read_csv(input_file,
                                     delimiter=delimiter,
                                     engine='python',
                                     header=None,
                                     dtype="double",
                                     skiprows=skip_rows)
        except Exception as e:
            self.exception = e
            return
        self.data_frame = data_frame
        down_sample = 1  # 默认不进行下采样

        npdata = np.array(data_frame)
        self.epochs = npdata.shape[0]
        self.width = npdata.shape[1]
        if max_points is not None:
            down_sample = int(self.epochs / max_points)
            if down_sample < 1:  # 禁止downsample 为0
                down_sample = 1
        if bins is None:
            bins = int(self.epochs / 3)  # 缺省bins为最大值/3
        down_sampled_data = npdata[::down_sample, :]
        self.wgs84 = WGS84()
        # 以开始历元为中心计算位置
        self.epochs = down_sampled_data.shape[0]
        self.width = down_sampled_data.shape[1]
        self.gps_week = self.times = None
        self.latitudes = self.longitudes = self.heights = None
        self.vn = self.ve = self.vd = None
        self.roll = self.pitch = self.heading = None
        self.p_q = self.h_q = None
        self.ns1 = self.ns2 = None
        self.coor = coor

        if 0 <= column_index[0] < self.width:
            self.gps_week = down_sampled_data[:, column_index[0]]
        if 0 <= column_index[1] < self.width:
            self.times = down_sampled_data[:, column_index[1]]
        if column_index[2] <= 0 or column_index[3] < 0 or column_index[4] < 0:
            return
        if self.coor == 0:
            self.latitudes = down_sampled_data[:, column_index[2]]
            self.longitudes = down_sampled_data[:, column_index[3]]
            self.heights = down_sampled_data[:, column_index[4]]
            self.vn = down_sampled_data[:, column_index[5]] if column_index[5] > 0 else None
            self.ve = down_sampled_data[:, column_index[6]] if column_index[6] > 0 else None
            self.vd = down_sampled_data[:, column_index[7]] if column_index[7] > 0 else None
            self.roll = down_sampled_data[:, column_index[8]] if column_index[8] > 0 else None
            self.pitch = down_sampled_data[:, column_index[9]] if column_index[9] > 0 else None
            self.heading = down_sampled_data[:, column_index[10]] if column_index[10] > 0 else None
        else:
            # E-frame to N-frame
            self.latitudes = np.zeros(self.epochs, dtype=np.double)
            self.longitudes = np.zeros(self.epochs, dtype=np.double)
            self.heights = np.zeros(self.epochs, dtype=np.double)
            # 位置
            for k in range(self.epochs):
                self.latitudes[k], self.longitudes[k], self.heights[k] = self.wgs84.ECEF_to_LLA(
                    down_sampled_data[k, column_index[2]], down_sampled_data[k, column_index[3]],
                    down_sampled_data[k, column_index[4]])
                # 速度
            if column_index[5] > 0 and column_index[6] > 0 and column_index[7] > 0:
                self.vn = np.zeros(self.epochs, dtype=np.double)
                self.ve = np.zeros(self.epochs, dtype=np.double)
                self.vd = np.zeros(self.epochs, dtype=np.double)
                for k in range(self.epochs):
                    Cne = self.wgs84.lla_to_Cne(self.latitudes[k], self.longitudes[k])
                    Ve = np.mat(down_sampled_data[k, [column_index[5], column_index[6], column_index[7]]]).transpose()
                    Vn = Cne * Ve
                    self.vn[k], self.ve[k], self.vd[k] = Vn[0], Vn[1], Vn[2]

        self.roll = down_sampled_data[:, column_index[8]] if 0 <= column_index[8] < self.width else None
        self.pitch = down_sampled_data[:, column_index[9]] if 0 <= column_index[9] < self.width else None
        self.heading = down_sampled_data[:, column_index[10]] if 0 <= column_index[10] < self.width else None
        self.p_q = down_sampled_data[:, column_index[11]] if 0 <= column_index[11] < self.width else None
        self.h_q = down_sampled_data[:, column_index[12]] if 0 <= column_index[12] < self.width else None
        self.ns1 = down_sampled_data[:, column_index[13]] if 0 <= column_index[13] < self.width else None
        self.ns2 = down_sampled_data[:, column_index[14]] if 0 <= column_index[14] < self.width else None

        if ref is None:
            self.center_lat = self.latitudes[0]
            self.center_lon = self.longitudes[0]
            self.center_height = self.heights[0]
        else:
            self.center_lat = ref[0]
            self.center_lon = ref[1]
            self.center_height = ref[2]
        if self.latitudes is not None and self.longitudes is not None and self.heights is not None:
            self.x_m = np.zeros(self.epochs, dtype=np.double)
            self.y_m = np.zeros(self.epochs, dtype=np.double)
            self.z_m = np.zeros(self.epochs, dtype=np.double)
            self.mean = np.zeros(3, dtype=np.double)
            for t in range(self.epochs):
                self.y_m[t] = self.wgs84.dN(self.latitudes[t] * np.pi / 180.0, self.center_lat * np.pi / 180.0)
                self.x_m[t] = self.wgs84.dE(self.latitudes[t] * np.pi / 180.0, self.longitudes[t] * np.pi / 180.0,
                                            self.center_lon * np.pi / 180.0)
                self.z_m[t] = self.heights[t] - self.center_height
                self.mean += np.array([self.x_m[t], self.y_m[t], self.z_m[t]]) / self.epochs
            self.x_hist, self.x_edges = np.histogram(np.abs(self.x_m - self.mean[0]), bins=bins)
            self.y_hist, self.y_edges = np.histogram(np.abs(self.y_m - self.mean[1]), bins=bins)
            self.z_hist, self.z_edges = np.histogram(np.abs(self.z_m - self.mean[2]), bins=bins)
            self.x_cdf = np.cumsum(self.x_hist)
            self.y_cdf = np.cumsum(self.y_hist)
            self.z_cdf = np.cumsum(self.z_hist)

        else:
            self.x_m = None
            self.y_m = None
            self.z_m = None
            self.mean = None
            self.x_hist, self.x_edges = None, None
            self.y_hist, self.y_edges = None, None
            self.z_hist, self.z_edges = None, None
            self.x_cdf = None
            self.y_cdf = None
            self.z_cdf = None

    def __ecef_to_n(self):

        for k in range(self.epochs):
            self.latitudes[k], self.longitudes[k], self.heights[k] = self.wgs84.ECEF_to_LLA(
                self.latitudes[k], self.longitudes[k], self.heights[k]
            )
        return


import struct


#
# class BinEncoder:
#     def __init__(self, filename, dtype=np.double):
#         self.mat_data = np.loadtxt(filename)
#
#     def save2file(self, outfile):
#         with open(outfile, 'wb') as f:
#             for line in self.mat_data:
#                 for data in line:
#                     b = struct.pack("<d", data)
#                     f.write(b)

#
# class BFrameConverter:
#     """
#     B-frame 前右下和右前上转换
#     """
#
#     def __init__(self, filename: str, filetype):
#         self.filename = filename
#         self.filetype = filetype
#         self.output_file = None
#
#     def __convert_frame(self, data):
#         new_data = np.zeros_like(data, "double")
#         new_data[:, 1] = data[:, 2]
#         new_data[:, 2] = data[:, 1]
#         new_data[:, 3] = -data[:, 3]
#         new_data[:, 4] = data[:, 5]
#         new_data[:, 5] = data[:, 4]
#         new_data[:, 6] = -data[:, 3]
#         return new_data
#
#     def convert(self):
#         if self.filetype == 0:  # bin
#             data = np.fromfile(self.filename, "double")
#             data = data.reshape([-1, 7])
#             '''F-R-D / R-F-U'''
#             self.output_file = self.filename + ".conv.bin"
#             with open(self.output_file, 'wb')as f:
#                 new_data = self.__convert_frame(data)
#                 for line in new_data:
#                     for data in line:
#                         b = struct.pack("<d", data)
#                         f.write(b)
#             return
#         if self.filetype == 1:
#             data = np.loadtxt(self.filename)
#             new_data = self.__convert_frame(data)
#             self.output_file = self.filename + ".conv.imutxt"
#             pd_data = pd.DataFrame(new_data)
#             pd_data.to_csv(self.output_file, sep=" ")


def testErrorAnalyse():
    infile = "/media/rebeater/存储/workspace/m39_0710/M39_20190710_01_lc.nav"
    reffile = "/media/rebeater/存储/workspace/m39_0710/M39_20190710_ref.nav"
    index = [0, 1, 2, 3, 4, 5, 6, 7, 8, 9, 10]


def convertGgga2Gps(gga_path, gps_path):
    """
    把GGA后缀转换成gps后缀的文件，添加其他文件
    :param gga_path:
    :param gps_path:
    :return:
    """
    Uere = 1
    gga_data = np.loadtxt(gga_path, skiprows=5)
    epoches = gga_data.shape[0]
    gps_data = np.zeros([epoches, 13])
    gps_data[:, 0] = gga_data[:, 1]
    gps_data[:, 1] = gga_data[:, 2]
    gps_data[:, 2] = gga_data[:, 3]
    gps_data[:, 3] = gga_data[:, 4]
    gps_data[:, 4:7] = -111
    gps_data[:, 7] = Uere * gga_data[:, 7]
    gps_data[:, 8] = Uere * gga_data[:, 7]
    gps_data[:, 9] = Uere * gga_data[:, 7]
    gps_data[:, 10:13] = -111
    np.savetxt(gps_path, gps_data, fmt="%18.15f")



def test():
    import pytz
    import datetime
    utc_tz = pytz.timezone('UTC')
    now = datetime.datetime.now(tz=utc_tz)
    cur_time =now + datetime.timedelta(seconds=18)  # GPST - UTC0 = 18s
    gps_0_time = datetime.datetime(1980, 1, 6, 0, 0, 0, 0,tzinfo=utc_tz)
    diff_time = cur_time - gps_0_time
    gps_week = np.floor(diff_time.days / 7)
    gps_second = 86400 * (diff_time.days % 7) + diff_time.seconds + diff_time.microseconds / 1000000
    print("now %s  %d  %d"%(str(now),gps_week,gps_second))
    # print("")
    return gps_week, gps_second


if __name__ == '__main__':
    while True:
        w,s= test()
        sleep(1)


